/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-24 06:47:41
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-11-30 11:25:45
 * @Description: ...
 * @Reference: ...
 */

#include "lci_slam/vilens/pipeline_online.h"

int NNN = 0;

void standardPclCallback(const sensor_msgs::PointCloud2::ConstPtr &lidar_msg) {
    NNN++;
    // std::cout << "lidar buffer size : " << NNN++ << std::endl;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "vilens_pipeline");

    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
                                   ros::console::levels::Info);

    std::string config_path = std::string(argv[1]);
    std::unique_ptr<vilens::onlinePipeline> online_pipeline_instance(
        new vilens::onlinePipeline(config_path));

    // ros::NodeHandle nh("~");
    // auto subLidar = nh.subscribe<sensor_msgs::PointCloud2>(
    //     "/ns2/velodyne_points", 200000, &standardPclCallback);

    // while (1) {
    //     std::cout << "lidar buffer size : " << NNN << std::endl;
    // }

    online_pipeline_instance->process();

    ros::spin();

    return 0;
}
